package Behaviors.Schemas.Motor;
import Behaviors.Schemas.Perception.*;
import Behaviors.Schemas.Behavior;
import Robot.Sensors.Sensor;
import Utils.*;
import java.awt.Color;

import Robot.*;
import javaclient3.*;

public class NxMSNavigationVFHNew extends Behavior{
    
    //Perception Behavior
    private NxPSDetectObstacles detect;
    private NxMSGoToPointNew target;
    private Position2DInterface pos;
    //maxRange deberia salir de las propiedades del sensor!!!
    private float maxRange = 0.6f;
    private float exp_param = 2.0f;

    private int numGrids = 60;
    private float angleStep;
    private float gridVals[];
    private float lastGridVals[];

    private int gridBinary[];

    //TODO: falta agregar una funcion para cambiar estos parametros dinamicamente
    //recordar ademas que estos parametros estan definidos para la distancia del
    //maximo rango de deteccion hacia el robot. no es distancia desde el robot hasta
    //el obstaculo!!!
    private float freeValleyThresh = 0.1f;
    private float occValleyThresh = 0.2f;

    private int smax = 15;
    private int minFreeCell = 6;


    public static final int MODE_LINEAR = 0;
    public static final int MODE_EXPONENTIAL = 1;
    private int atractorMode;

    private boolean showFrame;

    private Vector2d lastResultVector;
    private boolean enablePrivilegedDir = true;
    private boolean privilegedDirLeft = true;


    VectorFrame frame;
    
    public NxMSNavigationVFHNew(String name, Behavior detect, Behavior target,Position2DInterface myPos, int atractorMode, boolean showFrame) {

        super(name);
        this.detect=(NxPSDetectObstacles)detect;
        this.target = (NxMSGoToPointNew)target;
        this.pos = myPos;
        this.atractorMode = atractorMode;
        this.gridVals = new float[numGrids];
        this.lastGridVals = new float[numGrids];
        this.gridBinary = new int[numGrids];


        angleStep = (float)(2*Math.PI/numGrids);
        this.showFrame = showFrame;

        if(showFrame)
            configFrame();

        Vector2d targetVec = this.target.Value();
        lastResultVector = new Vector2d(targetVec.getMag(),targetVec.getAngle()+(float)Math.PI,Vector2d.MODE_MA);
        /*Class classOfdetect=detect.getClass();
       
        nombre.equals(classOfdetectclassOfdetect.getInterfaces()[0].getName());*/
        
    }
    public void configFrame()
    {
        frame = new VectorFrame("Histograma",VectorFrame.POLAR_COORD,10,numGrids);

        //draw circle that represents the robot
        frame.addStaticElement("robot", frame.CIRCLE_ELEMENT, Robot.radius, 0.0f, 0.0f, Color.RED);
        //draw circle that represents memory limit
        frame.addStaticElement("freeThreshold",frame.CIRCLE_ELEMENT, detect.getMaxRange() - freeValleyThresh, 0, 0,Color.ORANGE);
        frame.addStaticElement("occThreshold",frame.CIRCLE_ELEMENT, detect.getMaxRange() - occValleyThresh, 0, 0,Color.ORANGE);

        frame.addStaticElement("MaxRange",frame.CIRCLE_ELEMENT, detect.getMaxRange(), 0, 0,Color.GREEN);
        frame.addStaticElement("direction",frame.VECTOR_ELEMENT,0,1,0,Color.BLUE);
        //dont draw table
        frame.drawTable(false);
        frame.setVisible(true);
    }

    public void setMaxRange(float maxRange)
    {
        this.maxRange = maxRange;
    }
    
    public float getMaxRange()
    {
        return maxRange;
    }

    /**
     * Sets the thresholds used to mark valleys as occupied or not.
     * @param freeValleyT Limit distance from the robot to the obstacles, where
     * the obstacles in cells do not mark them as occupied.
     * @param occValleyT Limit distance from the robot to the obstacles, where
     * the obstacles in cells mark them as occupied.
     */
    public void setValleyTresh(float freeValleyT, float occValleyT)
    {
        this.freeValleyThresh = detect.getMaxRange() - freeValleyT;
        this.occValleyThresh = detect.getMaxRange() - occValleyT;


        if(showFrame)
        {
            this.frame.setVisible(false);
            this.frame = null;

            configFrame();
        }
    }

    public Vector2d Value()
    {

        //First read the obstacles
        Vector2d[] obstacles = detect.Value();
        
        //read orientation of robot
        double myDir = pos.getData().getPos().getPa();//float)pos.read(Sensor.READ_MODE_NO_WAIT).getPa();

        //float myDir = (float)pos.read(Sensor.READ_MODE_NO_WAIT).getPa();

        //System.out.println("OrientacionRobot: "+Math.toDegrees(myDir));
        //obtain direction to goal, and cell where this vector is located
        Vector2d targetVec = target.Value();

        Vector2d tVector = new Vector2d(targetVec);
        tVector = tVector.rotate(-myDir);
        float targetAngle = (float)tVector.getAngle();
        targetAngle = (float)Vector2d.angleCorrMPI2PI(targetAngle);
        int targetCell = (int)Math.floor(targetAngle/angleStep);
        if(targetCell<0)
            targetCell = numGrids + targetCell;


        float[] tempValues = new float[gridVals.length];

        if(showFrame)
        {
            frame.resetDynElements();
            frame.rotate((float)-myDir);
        }

        //map obstacles in cells of histogram grid
        for(int i=0;i<obstacles.length;i++)
        {

            //obstacle referenced to robot VCP
            obstacles[i] = obstacles[i].sub(Position2DHelper.getPosition(pos));
            obstacles[i] = obstacles[i].rotate(-myDir);

            float distToObst = (float)obstacles[i].getMag();
            
            //now assign value to polar grid
            float angle = (float)obstacles[i].getAngle();
            //correct angle to have it in the range [-PI, PI]
            angle = (float)Vector2d.angleCorrMPI2PI(angle);

            //calculate variation angle
            float varAng = (float)Math.atan2(detect.getObstSize(),distToObst);
            float angLeft = angle +varAng ;
            float angRight = angle-varAng;

            angLeft = (float)Vector2d.angleCorrMPI2PI(angLeft);
            angRight = (float)Vector2d.angleCorrMPI2PI(angRight);

            
            //define to which cells the obstacle belongs
            //the obstacle has three important points, center, left and right
            //each of them has to be evaluated

            int cell = (int) Math.floor(angle / angleStep);
            int cellLeft = (int) Math.floor(angLeft / angleStep);
            int cellRight = (int) Math.floor(angRight / angleStep);

            
            if(cell<0)
                cell = numGrids + cell;
            if(cellLeft<0)
                cellLeft = numGrids + cellLeft;
            if(cellRight<0)
                cellRight = numGrids + cellRight;


            if(showFrame)
                frame.addDynElement(Integer.toString(cell), frame.CIRCLE_ELEMENT, detect.getObstSize(), (float)obstacles[i].getX(), (float)obstacles[i].getY(), Color.RED);

            //read the max distance an obstacle can be detected
            float maxDist = detect.getMaxRange();

            //define how much cells the obstacle occupies
            int olCells = cellLeft - cell;
            int orCells = cell - cellRight;



            if(olCells<0)
            {
                olCells += numGrids;
            }

            if(orCells<0)
            {
                orCells += numGrids;
            }


            //float dist = distToObst;
            float dist = (maxDist - distToObst);

            //now mark the cells as occupied or not
            for(int j=0;j<=olCells;j++)
            {
                if(cell+j>= numGrids)
                {
                    if(tempValues[cell + j-numGrids]<dist)
                        tempValues[cell+j-numGrids] =  dist;
                }
                else
                {
                    if(tempValues[cell + j]<dist)
                        tempValues[cell+j] =  dist;
                }

            }

            for(int j=0;j<=orCells;j++)
            {

                if(cellRight+j>= numGrids)
                {
                    if(tempValues[cellRight + j-numGrids]<(dist))
                        tempValues[cellRight + j-numGrids] =  dist;
                }
                else
                {
                    if(tempValues[cellRight + j]<(dist))
                        tempValues[cellRight + j] =  dist;
                }
            }
        }
        lastGridVals = gridVals;
        gridVals = tempValues;

        int lastCell = -2;
        ValleyNew[] freeValleys = new ValleyNew[numGrids/2];
        int valleyCount = 0;

        //now identify free valleys based on the threshold
        for(int i= 0;i<gridVals.length;i++)
        {

            if(gridVals[i]>occValleyThresh || gridBinary[i] == 1)
            {
                if(showFrame)
                    frame.setOccupiedSector(i);
                gridBinary[i] = 1;
            }
            // free cells enter here
            if(gridVals[i]<freeValleyThresh || gridBinary[i] == 0)
            {
                gridBinary[i] = 0;
                if(showFrame)
                    frame.setFreeSector(i);
                if(i-1 == lastCell)
                {
                    lastCell = i;
                    freeValleys[valleyCount-1].setLastCell(i);
                }
                else
                    //new valley
                {
                    freeValleys[valleyCount] = new ValleyNew(numGrids,i,smax,(float)myDir);
                    freeValleys[valleyCount].setLastCell(i);
                    freeValleys[valleyCount].setGoalVector(targetVec);
                    valleyCount++;
                    lastCell = i;

                }
            }

        }


        
        //join first and last valleys, only of the first includes the first grid
        //and the last includes the last grid
        if(valleyCount >1)
        {
            if(freeValleys[0].getFirstCell() == 0 && freeValleys[valleyCount-1].getLastCell() == (numGrids-1))
            {
                freeValleys[0].setFirstCell(freeValleys[valleyCount-1].getFirstCell());
                freeValleys[valleyCount-1] = null;
                valleyCount--;
            }
        }


        //now we have identified all valleys. Choose the vector whose orientation
        //is closest to the target.
        
        float diffAngle = (float)Math.PI;

        Vector2d result = new Vector2d(targetVec.getMag(),targetVec.getAngle()+(float)Math.PI,Vector2d.MODE_MA);

        boolean resultUpdated = false;

        boolean secondSearch = false;
        //now select vector that best fit the direction to goal
        while(!resultUpdated)
        {
            /*if(!secondSearch)
                System.out.println("[1] Numero de sectores: "+valleyCount);
            else
                System.out.println("[2] Numero de sectores: "+valleyCount);
                */
            for(int i = 0; i<valleyCount;i++)
            {

                int w = freeValleys[i].getWidth();
                /*
                int fc = freeValleys[i].getFirstCell();
                int lc = freeValleys[i].getLastCell();
                float fa = freeValleys[i].getMinAngle();
                float la = freeValleys[i].getMaxAngle();
                */
                Vector2d[] vecs = freeValleys[i].getVectors();

                for(int j=0;j<vecs.length;j++)
                {
                    Vector2d v = vecs[j];

                    //System.out.print("S["+i+"] V["+j+"]: ");
                    //v.print(Vector2d.MODE_MA);
                    
                    //first check if there is enough space(more than minFreeCells free!)
                    if(w >= minFreeCell)
                    {

                        //Print vector in histogram frame
                        Vector2d resultFrame = new Vector2d(v);
                        resultFrame = resultFrame.rotate(-myDir);
                        if(showFrame)
                            frame.addDynElement("posibleDir", frame.VECTOR_ELEMENT, 0, (float)resultFrame.getX(), (float)resultFrame.getY(), Color.MAGENTA);

                        //if there are no obstacles, the resulting vector equals the
                        //vector to the goal
                        if(w == numGrids)
                        {
                            result = targetVec;
                            resultUpdated = true;
                        }
                        else
                        {
                            float angleResult = (float)Vector2d.angleCorrMPI2PI(result.getAngle() - targetVec.getAngle());
                            float angleValley = (float)Vector2d.angleCorrMPI2PI(v.getAngle() - targetVec.getAngle());

                            float angleToOrientation = (float)Math.abs(Vector2d.angleCorrMPI2PI(v.getAngle()-myDir));

                            if(Math.abs(angleValley) < Math.PI/9)
                            {
                                result = v;
                                resultUpdated = true;
                                //System.out.println("Muy cerca de la meta");
                            }

                            //System.out.println("angleResult: "+angleResult+" angleValley: "+angleValley);
                            //System.out.println("angleResult: "+Vector2d.angleCorrZero22PI(angleResult)+" angleValley: "+Vector2d.angleCorrZero22PI(angleValley));

                            //first check if vector of valley is closer to goal
                            //if(angleValley<angleResult)
                            //if(angleValley<0 && angleResult<0)
                            //    check = angleValley>angleResult;
                            //else if(angleValley)
                                
                            else if(Math.abs(angleValley)<Math.abs(angleResult))
                            {
                                
                                //then check if the difference of the previus chosen vector
                                //differs to much (in this case, 30 degrees) from
                                //this new vector.
                                //if(Math.abs(angleValley-angleResult)>3/2*angleStep)
                                //else//else if(Math.abs(Vector2d.angleCorrMPI2PI(angleValley-angleResult))>Math.PI/6)
                                //{
                                float diffAngleAct = (float)Math.abs(Vector2d.angleCorrMPI2PI(result.getAngle()-myDir));
                                float diffAngleNew = (float)Math.abs(Vector2d.angleCorrMPI2PI(v.getAngle()-myDir));

                                //check which one of the vectors is closer to the orientation of the robot.
                                if(diffAngleAct> diffAngleNew || !resultUpdated)
                                {
                                    result = v;
                                    diffAngle = angleToOrientation;
                                    resultUpdated = true;
                                    //System.out.println("Vectores diferentes, mas cerca de la actual orientacion");
                                    //result.print(Vector2d.MODE_MA);
                                }
                                    //else
                                    //{
                                        //System.out.println("No fue asignado el vector: diferentes pero mas alejado a la orientacion");
                                        //v.print(Vector2d.MODE_MA);
                                    //}
                                //}
                                //if the choice vectors are close enough, then choose the
                                //one that is closer to the orientation of the robot.
                                /*else
                                {
                                    //if(angleToOrientation<diffAngle)
                                    //first recalculate diffAngle (there is a new orientation!)
                                    diffAngle = Math.abs(Vector2d.angleCorrMPI2PI(result.getAngle()-myDir));
                                    //float diffAngleAct = Math.abs(Vector2d.angleCorrMPI2PI(result.getAngle()-myDir));
                                    //float diffAngleNew = Math.abs(Vector2d.angleCorrMPI2PI(v.getAngle()-myDir));
                                    
                                    if(Math.abs(Vector2d.angleCorrMPI2PI(diffAngle - angleToOrientation)) > angleStep)
                                    //if(diffAngleAct < diffAngleNew)
                                    {
                                     
                                        result = v;
                                        diffAngle = angleToOrientation;
                                        resultUpdated = true;
                                        System.out.println("Vectores similares, pero mas cercano a la orientacion");
                                        result.print(Vector2d.MODE_MA);

                                    
                                    }
                                    else
                                    {
                                        System.out.println("No fue asignado el vector: similares pero mas alejado a la orientacion");
                                        v.print(Vector2d.MODE_MA);
                                    }
                                }*/

                            }
                            else
                            {
                                //if not closer to goal, at least check if it is
                                //closer to the last direction
                                float diffAngleAct = (float)Math.abs(Vector2d.angleCorrMPI2PI(result.getAngle()-lastResultVector.getAngle()));
                                float diffAngleNew = (float)Math.abs(Vector2d.angleCorrMPI2PI(v.getAngle()-lastResultVector.getAngle()));

                                if(diffAngleAct>diffAngleNew)
                                {
                                    result = v;
                                    resultUpdated = true;
                                    //System.out.println("Vector mas alejado de la meta, pero mas cercano al anterior vector");

                                    //result.print(Vector2d.MODE_MA);

                                    //System.out.println("1");
                                }

                            }
                        }
                    }
                }
            }

            //resultUpdated = true;
            //no valley vector where chosen.
            
            if(!resultUpdated)
            {
                for(int i=0;i<valleyCount;i++)
                {
                    ValleyNew v = freeValleys[i];
                    if(v.getWidth()>minFreeCell)
                    {
                        result = new Vector2d(freeValleys[i].getVectors()[0]);
                        break;
                    }
                }
            }
            if(secondSearch)
                resultUpdated = true;
            else
                secondSearch = true;
             
        }

        //scale result vector with the magnitude of the target vector.
        result.setMag((float)targetVec.getMag());

        //System.out.print("Vector final: ");
        //result.print(Vector2d.MODE_MA);
        
        lastResultVector = new Vector2d(result);
        
        Vector2d resultFrame = new Vector2d(result);
        resultFrame = resultFrame.rotate(-myDir);

        if(showFrame)
        {
            frame.addDynElement("desiredDir", frame.VECTOR_ELEMENT, 0, (float)resultFrame.getX(), (float)resultFrame.getY(), Color.GREEN);
            frame.repaint();
        }
        
        //return total;
        if(enabled)
            return result;
        else
            return new Vector2d();

    }
    

    /*
    private Vector2d convertExponentialMode(Vector2d atractor){
        
        float x = atractor.getMag();
        
        //if object is in the closest area, then shutdown atractor
        if(x<maxRange)
        {
            float newMag =(float)(1 - Math.exp(-(maxRange-x)/exp_param));

            atractor.setMag(newMag);
            
        
        }
        else
            atractor.setMag(0);
        
        return atractor;
    
    }
    
    
    private Vector2d convertLinearMode(Vector2d atractor){

        //if obstacle in area of influence
        if(atractor.getMag()<maxRange){

            //System.out.println("Obst No. "+i+" is in influence area!");
            atractor.setMag(maxRange - atractor.getMag());

        }
        else
        {
            //System.out.println("Obst No. "+i+" is NOT in influence area!");
            atractor.setMag(0);
        }
        
        return atractor;

    
    }
    */

    //distancia a la cual el vector de velocidad se encuentra al 63% (0.63).
    public void setExponentialDistance(float x){
        
        //exp_param = (float)-Math.log(1 - Math.sqrt(2)/2.0f) / x;
        exp_param = (float)maxRange-x;

    
    }
}
class ValleyNew
{
    private int firstCell;
    private int lastCell;
    private int numGrids;
    private int smax;
    private float orientation;
    private float angleDiff;
    private Vector2d goal = null;

    public ValleyNew(int numGrids, int firstCell, int smax, float orientation){
        this.firstCell = firstCell;
        this.numGrids = numGrids;
        this.smax = smax;
        this.orientation = orientation;
        this.angleDiff = (float) (2*Math.PI / numGrids);
    }

    public void setLastCell(int lastCell)
    {
        this.lastCell = lastCell;
    }

    public void setGoalVector(Vector2d goal)
    {
        this.goal = goal;
    }

    public void setFirstCell(int firstCell)
    {
        this.firstCell = firstCell;
    }
    public int getLastCell()
    {
        return lastCell;
    }

    public int getFirstCell()
    {
        return firstCell;
    }

    public float getMinAngle()
    {
        return firstCell * angleDiff;
    }

    public float getMaxAngle()
    {
        return (lastCell+1) * angleDiff;
    }

    public int getWidth()
    {
        if(firstCell>lastCell)
            return lastCell - firstCell + numGrids + 1;
        else
            return lastCell - firstCell + 1;
    }

    public boolean isCellFree(int cell)
    {
        if(cell == firstCell || cell == lastCell)
            return true;
        if(firstCell<lastCell)
            return cell>firstCell && cell<lastCell;
        else
        {
            if(cell>firstCell)
                return true;
            else if(cell<lastCell)
                return true;
        }
        return false;
    }

    public Vector2d[] getVectors()
    {
        Vector2d[] result;
        //no obstacle was detected. steering is 0.
        if (getWidth() == numGrids)
        {
            result = new Vector2d[1];
            result[0] = new Vector2d(0,0,Vector2d.MODE_MA);
            return result;
        }
        else
        {
         
            if(getWidth() > smax)
            {

                result = new Vector2d[2];


                float kn1 = firstCell * angleDiff;
                float kn2 = (lastCell+1) * angleDiff;


                float kf1;
                float kf2;

                
                if((firstCell+smax) >= numGrids)
                    kf1 = (firstCell+smax-numGrids) * angleDiff;
                else
                    kf1 = (firstCell+smax) * angleDiff;

                if((lastCell+1-smax) < 0)
                    kf2 = (lastCell-smax+numGrids+1) * angleDiff;
                else
                    kf2 = (lastCell-smax+1) * angleDiff;



//                System.out.println("kn: "+Math.toDegrees(kn)+" kf "+Math.toDegrees(kf));
//                System.out.println("kn: "+(kn)+" kf "+(kf));
                //kf = Vector2d.angleCorrection(kf);

                float angle1 = (kn1 + kf1);
                float angle2 = (kn2 + kf2);


                if(kn1 > kf1)
                    angle1 -= 2*Math.PI;


                if(kn2 < kf2)
                    angle2 -= 2*Math.PI;


                angle1 /= 2;
                angle2 /= 2;

//                System.out.println("angle: "+angle);
                angle1 += orientation;
                angle2 += orientation;


                //System.out.println("kn: "+kn+" kf: "+kf+" orientacion: "+orientation+ "angle: "+angle);

                //calculate vector and correct angle in order to have it between [-PI, PI]
                result[0] = new Vector2d(1,angle1,Vector2d.MODE_MA);
                result[1] = new Vector2d(1,angle2,Vector2d.MODE_MA);


                result[0].setAngle((float)Vector2d.angleCorrMPI2PI(result[0].getAngle()));
                result[1].setAngle((float)Vector2d.angleCorrMPI2PI(result[1].getAngle()));

                //now compare vectors with goal vector
                float finalAng1 = (float)result[0].getAngle();
                float finalAng2 = (float)result[1].getAngle();
                float goalAng = (float)goal.getAngle();

                goalAng -= finalAng1;
                finalAng2 -= finalAng1;

                goalAng = (float)Vector2d.angleCorrZero22PI(goalAng);
                finalAng2 = (float)Vector2d.angleCorrZero22PI(finalAng2);

                //if goal vector is between the two recommended vectors, add
                //goal vector to the vectors list.
                if(goalAng <= finalAng2)
                {

                    Vector2d[] tempResult = new Vector2d[3];
                    tempResult[0] = result[0];
                    tempResult[1] = result[1];
                    tempResult[2] = goal;

                    result = tempResult;
                }

                return result;
            }
            else
            {
                result = new Vector2d[1];
                //System.out.println("width < Smax");
                float kn = firstCell * angleDiff;
                
                float kf = (lastCell+1) * angleDiff;


                float angle = (kn + kf);

                if(firstCell>lastCell)
                    angle -= 2*Math.PI;

                angle /= 2;
                angle += orientation;

                //System.out.println("kn: "+kn+" kf: "+kf+" angle: "+angle);

                //calculate vector and correct angle in order to have it between [-PI, PI]
                result[0] = new Vector2d(1,angle,Vector2d.MODE_MA);
                result[0].setAngle((float)Vector2d.angleCorrMPI2PI(result[0].getAngle()));
                return result;

            }

        }

    }

}
